#include "opencv2/core/core.hpp"
#include "opencv2/imgproc/imgproc.hpp"
#include "opencv2/highgui/highgui.hpp"
#include "opencv2/video/video.hpp"
#include "visionLanding.hpp"
#include <stdio.h>
#include <string>
#include <iostream>
#include <fstream>

using namespace std;
using namespace cv;

int main()
{

	xyVision::visionLanding sample("config_gy1.ini");
	cv::VideoWriter writer("slam_result25.avi",CV_FOURCC('M','J','P','G'), 10, Size(640, 360));
	namedWindow("result");
	// namedWindow("bi_red");
	// namedWindow("bi_blue");
	// namedWindow("bi_yellow");
	// namedWindow("bi_purple");
	// namedWindow("bi_plus");
 //    namedWindow("bi_plus_warp");
	int k = 0;
	Mat img, rectified;
    //VideoCapture cap(0);
	//cap.release();
	//cap.set(CV_CAP_PROP_FRAME_WIDTH, 1280);
    //cap.set(CV_CAP_PROP_FRAME_HEIGHT, 720);
    ofstream file("loc.txt");
	while(1)
	{
		if (k > 2000)
			break;

		char ch[10];
		sprintf(ch, "%.5d", k);
		//string file_name = "/home/ubuntu/vision/slam/data/image0113/2017_01_13_03_24_30_071-101-5/" + string(ch) + ".jpg";
		string file_name = "/home/ubuntu/vision/slam/data/gc2_test3/" + string(ch) + ".jpg";
        //string file_name = "test18.jpg";
		img = imread(file_name);
	    //cap >> img;
		//cout << "img size" << img.rows << " " << img.cols <<"channels" << img.channels() <<endl;
		cout << k << endl;
		//imwrite("img.jpg", img);
		//img = imread("/save_map/build/bin/img1225/00001.jpg");
		k = k + 1;
		if (img.empty())
		{
			cout << "img empty" << endl;
			break;
		}
		if (waitKey(20) > 0)
		{
			break;
		}
        Point3f loc;

        loc = sample.getCurrentLoc(img, 1000);

		//if(k > 10)
		//	sample.setBoardVersion("1.3");
		rectified = sample.rectifiedImg;
        file << "k=" << k << " x=" << loc.x << " y=" << loc.y << " z=" << loc.z << endl;
		//cv::imwrite("F:\\lai\\projects\\xunyi\\data\\landing\\image-86-rect\\" + string(ch) + ".jpg", rectified);
		if (sample.isDetected)
		{
            cout << "Detected!" << endl;
            cout << "Loc = " << loc.x << " " <<loc.y << " " << loc.z << endl;
			//if (sample.height_state == xyVision::HEIGHT_HIGH)
			//{
			//	// show large square
			//	for (int i = 0; i < sample.largeBoardInfo.numSquareBoard; ++i)
			//	{
			//		Point2f vertices[4];
			//		sample.largeBoardInfo.detectedBoxes[i].points(vertices);
			//		line(rectified, Point((int)vertices[0].x, (int)vertices[0].y), Point(int(vertices[1].x), (int)vertices[1].y), Scalar(255, 0, 0), 3);
			//		line(rectified, Point((int)vertices[1].x, (int)vertices[1].y), Point(int(vertices[2].x), (int)vertices[2].y), Scalar(255, 0, 0), 3);
			//		line(rectified, Point((int)vertices[2].x, (int)vertices[2].y), Point(int(vertices[3].x), (int)vertices[3].y), Scalar(255, 0, 0), 3);
			//		line(rectified, Point((int)vertices[3].x, (int)vertices[3].y), Point(int(vertices[0].x), (int)vertices[0].y), Scalar(255, 0, 0), 3);
			//	}
   //             char ch[30];
   //             sprintf(ch, "%.2f", loc.x);
   //             putText(rectified, "X = " + string(ch), Point(200, 30), FONT_HERSHEY_SIMPLEX, 1, Scalar(255, 255, 255), 2);
   //             sprintf(ch, "%.2f", loc.y);
   //             putText(rectified, "Y = " + string(ch), Point(200, 100), FONT_HERSHEY_SIMPLEX, 1, Scalar(255, 255, 255), 2);
   //             sprintf(ch, "%.2f", loc.z);
   //             putText(rectified, "Z = " + string(ch), Point(200, 170), FONT_HERSHEY_SIMPLEX, 1, Scalar(255, 255, 255), 2);
   //             sprintf(ch, "%d", sample.frameCounter);
   //             putText(rectified, "FrameCounter = " + string(ch), Point(200, 250), FONT_HERSHEY_SIMPLEX, 1, Scalar(255, 255, 255), 2);
			//}
   //         else if (sample.height_state == HEIGHT_LOW)
   //         {
   //             // show
   //         }

            // show center
            Point3f plane_p(loc.x/loc.z, loc.y/loc.z, 1.0f);
            Point3f plane_i = sample.cameraInfo.newCameraMatrix * plane_p;
            //Matx33f K_dummy = Matx33f(650.f, 0.f, 320.f, 0.f, 650.f, 180.f, 0.f, 0.f, 1.f);
            //Point3f plane_i = K_dummy * plane_p;
            cv::circle(rectified, Point((int)plane_i.x, (int)plane_i.y), 15, Scalar(255, 0, 0), 3);
            char ch[30];
			sprintf(ch, "%.2f", loc.x);
			putText(rectified, "X = " + string(ch), Point(200, 30), FONT_HERSHEY_SIMPLEX, 1, Scalar(255, 255, 255), 2);
			sprintf(ch, "%.2f", loc.y);
			putText(rectified, "Y = " + string(ch), Point(200, 100), FONT_HERSHEY_SIMPLEX, 1, Scalar(255, 255, 255), 2);
			sprintf(ch, "%.2f", loc.z);
			putText(rectified, "Z = " + string(ch), Point(200, 170), FONT_HERSHEY_SIMPLEX, 1, Scalar(255, 255, 255), 2);
            sprintf(ch, "%d", sample.frameCounter);
            putText(rectified, "FrameCounter = " + string(ch), Point(200, 250), FONT_HERSHEY_SIMPLEX, 1, Scalar(255, 255, 255), 2);
		}
        else
        {
            cout << "Do not detected" << endl;
            putText(rectified, "do not detect! ", Point(400, 30), FONT_HERSHEY_SIMPLEX, 1, Scalar(255, 255, 255), 1);
            char ch[30];
            sprintf(ch, "%d", sample.frameCounter);
            putText(rectified, "FrameCounter = " + string(ch), Point(200, 250), FONT_HERSHEY_SIMPLEX, 1, Scalar(255, 255, 255), 2);
        }
        imshow("result", rectified);
        //writer << rectified;
        waitKey(200);
	}
	//cap.release();
    file.close();
}
